#include <tT.h>
#include <env.h>
#include <avr/interrupt.h>

#include "can.h"
#include "honda.h"
#include "front.h"
#include "rear.h"
#include "dashboard.h"
#include "adcinput.h"

void init(void) {
	
	CLKPR = (1<<CLKPCE); 
	CLKPR = 0;				// 1 prescaling 16MHz
	
	//Clearing timer0 may have solved a bug on PCINT13
	TCCR0A = 0;
	TCCR0B = 0;
	TIMSK0 = 0;
	
	TT_SYNC(&can, canInit, TT_ARGS_NONE);
	
	//DDRD = 0;
	//PORTD = (1<<PIND5)|(1<<PIND6)|(1<<PIND7); //This is to enale node IDs
	
	
	//TT_SYNC(&dashboard,dashboardInit, TT_ARGS_NONE);
	//TT_SYNC(&ADCinput, ADCInit,TT_ARGS_NONE);
	//TT_SYNC(&honda, hondaInit,TT_ARGS_NONE);
	//TT_SYNC(&rear, rearInit, TT_ARGS_NONE);
	TT_SYNC(&front,frontInit,TT_ARGS_NONE);
	/*switch ((PIND)>>5){
		case 0:
			ENV_DEBUG("Case is 0, And this is the front node\n\r");
			TT_SYNC(&front, frontInit, TT_ARGS_NONE);
			break;
		case 1:
			ENV_DEBUG("Case is 1, and this is the rear node\n\r");
			TT_SYNC(&rear, rearInit, TT_ARGS_NONE);
			break;
		case 2:
			ENV_DEBUG("Case is 2, and this is the honda node\n\r");
			TT_SYNC(&honda,hondaInit,TT_ARGS_NONE);
			TT_SYNC(&ADCinput, ADCInit,TT_ARGS_NONE);
			break;
		case 4:
			ENV_DEBUG("Case is 4, and this is the dashoard  \n\r");
			TT_SYNC(&dashboard,dashboardInit, TT_ARGS_NONE);
			TT_SYNC(&ADCinput, ADCInit,TT_ARGS_NONE);
			break;
		default:
			ENV_DEBUG("Case is Default \n\r");
			break;
	}*/
	
	//Setup WDT
	//WDTCSR = (1<<WDE) | (1<<WDCE);
 	//WDTCSR = (1<<WDE) | (1<<WDP2) | (1<<WDP1) | (1<<WDP0);
	
	//Reset the WDT
	//asm("WDR"); 
	
	can_message_t msg;
	msg.id = 500;
	msg.length = 0;
	TT_SYNC(&can, canSend, &msg);
}

ENV_STARTUP(init);
